// Open loop motor control example
#include <SimpleFOC.h>


// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number);
BLDCMotor motor = BLDCMotor(11);
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);

// Stepper motor & driver instance
//StepperMotor motor = StepperMotor(50);
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);


//target variable
float target_velocity = 0;

// instantiate the commander
Commander command = Commander(Serial);

void doTarget(char *cmd)
{ command.scalar(&target_velocity, cmd); }

void setup()
{

    // driver config
    // power supply voltage [V]
    driver.voltage_power_supply = 12;
    driver.init();
    // link the motor and the driver
    motor.linkDriver(&driver);

    // limiting motor movements
    motor.voltage_limit = 3;   // [V]
    motor.velocity_limit = 5; // [rad/s] cca 50rpm

    // open loop control config
    motor.controller = MotionControlType::velocity_openloop;

    // init motor hardware
    motor.init();

    // add target command T
    command.add('T', doTarget, "target velocity");

    Serial.begin(115200);
    Serial.println("Motor ready!");
    Serial.println("Set target velocity [rad/s]");
    _delay(1000);
}

void loop()
{

    // open loop velocity movement
    // using motor.voltage_limit and motor.velocity_limit
    motor.move(target_velocity);

    // user communication
    command.run();
}
